#include "Hchassis.h"
const double k_radical = 1;    // 底盘猛烈程度
const double ki = 1 / k_radical; // 摇杆控制上升增长率
/*
摇杆数值与输出动力的图像
y=k*(x-bx).^ki;
ki=1 图像如下；
//      输出动力如下：ki=1
//                |
//                |           /
//                |          /
//                |         /
//     -blind     |        /
//  - ----————————————————-------------->
//       /        |    blind          摇杆数值
//      /         |
//     /          |
//    /           |
//                |
//                |
k_radical=1, ki=1:匀速上升；
k_radical<1, ki>1:缓慢上升；
k_radical>1, ki<1:急速上升；
*/

// #################################
int auto_hold_trigger_dst = 25; // mm
#define time_delay_to_check 250 // ms

void Hchassis::handle(double chx_forward, double chx_turn, ctrlMode mode,
                      double max_forward_pct, double max_turn_pct,
                      bool auto_hold) {
  // 实验：叠加陀螺仪旋转速率
  double auto_ctrl_pwr = 0;
  extern double get_gyro_rate();
  double rate = -get_gyro_rate();
  if (abs(chx_turn) < 15 && abs(rate) > 5) {
    double auto_turn_brake_pwr = 0.8 * rate;
    abs_max_limit(auto_turn_brake_pwr, 15);
    auto_ctrl_pwr = auto_turn_brake_pwr;
  }
  ///////////////////

  static bool enc_recorded = false;
  static timer t;

  ActionUnit::set_all_units_motor_stop_status_on_working(
      abs(chx_forward) > 20 || abs(chx_turn) > 20);
  ////////////////////////////////////////////////////////////////////////
#define AXIS_BLIND_AREA 0
  zero_out_blind(chx_forward, AXIS_BLIND_AREA);
  zero_out_blind(chx_turn, AXIS_BLIND_AREA);

  // 摇杆数值处理，稳定小数值归零
  chx_forward = process.zero_out_stable_small_value(
      chx_forward, JoystickValueProcess::timer_index_forward);
  chx_turn = process.zero_out_stable_small_value(
      chx_turn, JoystickValueProcess::timer_index_turn);

  // 数学函数模型:  y=k*(x-bx).^ki;
  const double bx = AXIS_BLIND_AREA;
  const double k = 100 / pow((100 - bx), ki);

  // forward
  double chx_forward_positive = abs(chx_forward); // 正值
  int sgn_forward = sgn(chx_forward);
  if (chx_forward_positive < AXIS_BLIND_AREA) {
    chx_forward = 0;
  } else {
    chx_forward = k * pow((chx_forward_positive - bx), ki); // y=k*(x-bx).^ki;
    chx_forward = sgn_forward * chx_forward;
  }
  // turn
  double chx_turn_positive = abs(chx_turn); // 正值
  int sgn_turn = sgn(chx_turn);
  if (chx_turn_positive < AXIS_BLIND_AREA) {
    chx_turn = 0;
  } else {
    chx_turn = k * pow((chx_turn_positive - bx), ki); // y=k*(x-bx).^ki;
    chx_turn = sgn_turn * chx_turn;
  }

  ////////////////////////////////////////////////////////////////////////
  // 底盘功率输出
  if (chx_forward || chx_turn) {
    set_stopping(set_brake_type); // 恢复设置的停转模式
    if (mode == m_velocity)
      FT_set_speed(chx_forward, chx_turn );
    else
      FT_set_voltage(chx_forward, chx_turn);
    // 在控制时,不断归零参数
    enc_recorded = false;
    auto_hold_turned_on = false;
    t.reset(); // 不断归零计时
  } else {
    stop();
    if (auto_hold && !ActionUnit::is_all_units_relaxing()) {
      static long stop_dst_L = 0, stop_dst_R = 0;
      if (t.time() > time_delay_to_check &&
          !enc_recorded) { // 记录刚停转时的编码器
        stop_dst_L = get_dist_mm(-1);
        stop_dst_R = get_dist_mm(1);
        enc_recorded = true;
      }

      // 超过一定时间，并且被推动
      // if (enc_recorded &&
      //     (abs(stop_dst_L - get_dist_mm(-1)) > auto_hold_trigger_dst ||
      //      abs(stop_dst_R - get_dist_mm(1)) > auto_hold_trigger_dst)) {
      //   if (!auto_hold_turned_on) { // 未开启
      //     auto_hold_turned_on = true;
      //     set_stopping_without_saving(hold); //
      //   }
      // } // auto_hold on
    }   // auto_hold function
  }
}
